using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;

namespace Kinematic
{
    struct Collision
    {
        public Vector3 point;
        public Vector3 normal;

        public Collision(int A)
        {
            point = Vector3.Zero;
            normal = Vector3.Zero;
        }

    }

    public class Class1
    {

        public Class1()
        {
            SetRamdom();
        }
        public Random Rand;
        //Variables
        float avoidDistance = 50.0f;
        float lookahead = 100.0f;

        // Velocidad maxima que puede alcanzar
        public float maxSpeed;

        // Radio que considera si funciono
        public float radius;

        public float timeToTarget;

        //el angulo maximo de rotacion permitido en wander
        public float maxRotation;


        //dynamic
        //Acerelacion Maxima que puede alcanzar
        public float maxAceleration;

        // Radio dentro del cual considera que ya llego al objetivo
        public float targetRadius;

        //Radio que comienza a disminuir la velocidad por aproximarse al objetivo
        public float slowRadius;

        // Aling
        float maxAngularAcceleration;


        // maximo tiempo de prediccion
        public float maxPrediction;
        public float threshold;
        public float decayCoefficient;


        public void SetRamdom()
        {
            this.Rand = new Random(System.DateTime.Now.Millisecond); ;
        }

        //Setiar Kinematic Seek
        public void SetKS()
        {
            maxSpeed = 20.0f;
        }
        //Personaje intenta alcanzar al target y lo busca eternamente nunca se detiene (con Velocidad)
        public SteeringOutput KinematicSeek(Kinematic Personaje, Kinematic Target)
        {
            KinematicSteeringOutput Steering = new KinematicSteeringOutput();

            Steering.Velocity = Target.Position - Personaje.Position;
            Steering.Velocity.Normalize();
            Steering.Velocity *= maxSpeed;

            Personaje.Orientation = getNewOrientation(Personaje, Steering.Velocity);

            Steering.Rotation = 0;

            return new SteeringOutput(Steering);

        }

        //Setiar Dinamyc Seek
        public void SetDS()
        {
            maxAceleration = 15.0f;
        }
        //Personaje intenta alcanzar al target y lo busca eternamente nunca se detiene (con Aceleracion)
        public SteeringOutput DynamicSeek(Kinematic Personaje, Kinematic Target)
        {
            SteeringOutput Steering = new SteeringOutput();

            Steering.linear = Target.Position - Personaje.Position;
            Steering.linear.Normalize();
            Steering.linear *= maxAceleration;

            Steering.agular = 0;

            

            return Steering;

        }

        //Setiar Kinematic Arrive
        public void SetKA()
        {
            timeToTarget = 0.1f;
            maxSpeed = 20.0f;

        }
        //personaje intenta alcanzar a target y se detiene al conseguirlo (con velocidad)
        public SteeringOutput KinematicArrive(Kinematic Personaje, Kinematic Target)
        {
            KinematicSteeringOutput Steering = new KinematicSteeringOutput();

            Steering.Velocity = Target.Position - Personaje.Position;

            if (Steering.Velocity.Length() < radius)
            {
                return new SteeringOutput();
            }

            Steering.Velocity /= timeToTarget;

            if (Steering.Velocity.Length() > maxSpeed)
            {
                Steering.Velocity.Normalize();
                Steering.Velocity *= maxSpeed;
            }
            Personaje.Orientation = getNewOrientation(Personaje, Steering.Velocity);

            Steering.Rotation = 0;

            return new SteeringOutput(Steering);

        }

        //Setiar Dynamic Arrive
        public void SetDA()
        {
            maxSpeed = 20.0f;
            maxAceleration = 5.0f;
            slowRadius = 8.0f;
            timeToTarget = 0.8f;
            targetRadius = 5.0f;
        }
        //personaje intenta alcanzar a target y se detiene al conseguirlo (con aceleracion)
        public SteeringOutput DynamicArrive(Kinematic Personaje, Kinematic Target)
        {
            SteeringOutput Steering = new SteeringOutput();

            Vector3 direction = Target.Position - Personaje.Position;
            float distance = direction.Length();
            float targetSpeed;

            if (distance < targetRadius)
                return Steering;

            if (distance > slowRadius)
                targetSpeed = maxSpeed;
            else
                targetSpeed = maxSpeed * distance / slowRadius;

            Vector3 targetVelocity = direction;
            targetVelocity.Normalize();
            targetVelocity *= targetSpeed;

            Steering.linear = targetVelocity - Personaje.Velocity;
            Steering.linear /= timeToTarget;


            if (Steering.linear.Length() > maxAceleration)
            {
                Steering.linear.Normalize();
                Steering.linear *= maxAceleration;
            }

            Steering.agular = 0;

            return Steering;

        }

        //Setiar Kinematic Flee
        public void SetKF()
        {
            maxSpeed = 20.0f;
        }
        //Personaje intenta alcanzar al target y lo busca eternamente nunca se detiene (con Velocidad)
        public SteeringOutput KinematicFlee(Kinematic Personaje, Kinematic Target)
        {
            KinematicSteeringOutput Steering = new KinematicSteeringOutput();

            Steering.Velocity = Personaje.Position - Target.Position;
            Steering.Velocity.Normalize();
            Steering.Velocity *= maxSpeed;

            Personaje.Orientation = getNewOrientation(Personaje, Steering.Velocity);

            Steering.Rotation = 0;

            return new SteeringOutput(Steering);

        }

        //Setiar Dinamyc Flee
        public void SetDF()
        {
            maxAceleration = 15.0f;
        }
        //Personaje intenta alcanzar al target y lo busca eternamente nunca se detiene (con Aceleracion)
        public SteeringOutput DynamicFlee(Kinematic Personaje, Kinematic Target)
        {
            SteeringOutput Steering = new SteeringOutput();

            Steering.linear = Personaje.Position - Target.Position;
            Steering.linear.Normalize();
            Steering.linear *= maxAceleration;

            Steering.agular = 0;

            return Steering;

        }

        //Setiar Kinematic Leave
        public void SetKL()
        {
            timeToTarget = 0.1f;
            maxSpeed = 20.0f;

        }
        //personaje intenta alcanzar a target y se detiene al conseguirlo (con velocidad)
        public SteeringOutput KinematicLeave(Kinematic Personaje, Kinematic Target)
        {
            KinematicSteeringOutput Steering = new KinematicSteeringOutput();

            Steering.Velocity = Personaje.Position - Target.Position;

            if (Steering.Velocity.Length() < radius)
            {
                return new SteeringOutput();
            }

            Steering.Velocity /= timeToTarget;

            if (Steering.Velocity.Length() > maxSpeed)
            {
                Steering.Velocity.Normalize();
                Steering.Velocity *= maxSpeed;
            }
            Personaje.Orientation = getNewOrientation(Personaje, Steering.Velocity);

            Steering.Rotation = 0;

            return new SteeringOutput(Steering);

        }

        //Setiar Dynamic Leave
        public void SetDL()
        {
            maxSpeed = 20.0f;
            maxAceleration = 5.0f;
            slowRadius = 80.0f;
            timeToTarget = 0.1f;
            targetRadius = 10.0f;
        }
        //personaje intenta alcanzar a target y se detiene al conseguirlo (con aceleracion)
        public SteeringOutput DynamicLeave(Kinematic Personaje, Kinematic Target)
        {
            SteeringOutput Steering = new SteeringOutput();

            Vector3 direction = Personaje.Position - Target.Position;
            float distance = direction.Length();
            float targetSpeed;

            if (distance < targetRadius)
                return Steering;

            if (distance > slowRadius)
                targetSpeed = maxSpeed;
            else
                targetSpeed = maxSpeed * distance / slowRadius;

            Vector3 targetVelocity = direction;
            targetVelocity.Normalize();
            targetVelocity *= targetSpeed;

            Steering.linear = targetVelocity - Personaje.Velocity;
            Steering.linear /= timeToTarget;


            if (Steering.linear.Length() > maxAceleration)
            {
                Steering.linear.Normalize();
                Steering.linear *= maxAceleration;
            }

            Steering.agular = 0;

            return Steering;

        }

        public SteeringOutput Detener(Kinematic Character)
        {
            SteeringOutput Steering = new SteeringOutput();
            Steering.linear = -Character.Velocity;
            return Steering;
        }

        //Setiar Kinematic Wander
        public void SetKW()
        {
            maxSpeed = 200.0f;
            maxRotation = MathHelper.PiOver4;
        }
        // movimiento al azar con rotacion incluida
        public SteeringOutput KinematicWander(Kinematic Personaje)
        {
            KinematicSteeringOutput Steering = new KinematicSteeringOutput();

            Vector3 Aux = asVector(Personaje.Orientation);

            Steering.Velocity = maxSpeed * Aux;

            Steering.Rotation = (float)(randmBinomial() * maxRotation * 0.2);

            return new SteeringOutput(Steering);

        }

        //Setiar Velocity Match
        public void SetVM()
        {
            timeToTarget = 0.1f;
            maxAceleration = 5.0f;
        }
        //character iguala su velocidad con la de target
        public SteeringOutput VelocityMatch(Kinematic Character, Kinematic Target)
        {
            SteeringOutput Steering = new SteeringOutput();

            Steering.linear = Target.Velocity - Character.Velocity;
            Steering.linear /= timeToTarget;

            if (Steering.linear.Length() > maxAceleration)
            {
                Steering.linear.Normalize();
                Steering.linear *= maxAceleration;
            }

            Steering.agular = 0;

            return Steering;
        }

        //Setiar Pursue
        public void SetP()
        {
            maxPrediction = 5.0f;
            SetDS();
        }
        //character persigue a target
        public SteeringOutput Pursue(Kinematic Character, Kinematic Target)
        {
            Vector3 direction = Target.Position - Character.Position;
            float distance = direction.Length();
            float speed = Character.Velocity.Length();

            float prediction;

            if (speed <= distance / maxPrediction)
                prediction = maxPrediction;
            else
                prediction = distance / speed;

            Kinematic Aux = Target.Clone();

            Aux.Position += Target.Velocity * prediction;

            return DynamicSeek(Character, Aux);

        }

        //Setiar Evade
        public void SetE()
        {
            maxPrediction = 5.0f;
            SetDS();
        }
        //character persigue a target
        public SteeringOutput Evade(Kinematic Character, Kinematic Target)
        {
            Vector3 direction = Target.Position - Character.Position;
            float distance = direction.Length();
            float speed = Character.Velocity.Length();

            float prediction;

            if (speed <= distance / maxPrediction)
                prediction = maxPrediction;
            else
                prediction = distance / speed;

            Kinematic Aux = Target.Clone();

            Aux.Position += Target.Velocity * prediction;

            return DynamicFlee(Character, Aux);

        }

        //Setiar Sepatation
        public void SetS()
        {
            threshold = 20.0f; //Umbral de separacion
            decayCoefficient = 10.0f;
            maxAceleration = 5.0f;
        }
        //evita que character se monte sobre cualquiera que este en lista de targets
        public SteeringOutput Separation(Kinematic Character, List<Kinematic> Targets)
        {
            SteeringOutput Steering = new SteeringOutput();
            float strength = 0.0f;
            float distance;
            Vector3 direction;

            foreach (Kinematic Target in Targets)
            {
                direction = Character.Position - Target.Position;
                distance = direction.Length();

                if (distance > 0 && distance < threshold)
                {
                    strength = Math.Min(decayCoefficient * distance * distance, maxAceleration);
                    direction.Normalize();
                    Steering.linear = strength * direction;
                }
            }
            return Steering;
        }

        //Setiar CollisionAvoidance
        public void SetCA()
        {
            radius = 20.0f;
            maxAceleration = 5.0f;
        }
        //Supuestamente evita coliciones aun no me funca
        public SteeringOutput CollisionAvoidance(Kinematic Character, List<Kinematic> Targets)
        {
            float shortestTime = float.MaxValue;

            Kinematic firstTarget = null;
            float firstMinSeparation = -1.0f;
            float firstDistance = -1.0f;
            Vector3 firstRelativePos = Vector3.Zero;
            Vector3 firstRelativeVel = Vector3.Zero;

            SteeringOutput Steering = new SteeringOutput();

            Vector3 relativePos = Vector3.Zero;
            Vector3 relativeVel;
            float relativeSpeed;
            float timeToCollision;
            float distance;
            Ray AA = new Ray(Character.Position, asVector(Character.Orientation));

            foreach (Kinematic Target in Targets)
            {
                relativePos = Character.Position - Target.Position;
                relativeVel = Character.Velocity - Target.Velocity;
                relativeSpeed = relativeVel.Length();


                float? Aux = AA.Intersects(new BoundingSphere(Target.Position, radius));

                if (Aux.HasValue)
                {
                    distance = Aux.Value;
                }
                else
                {
                    distance = -1;
                }


                timeToCollision = distance / relativeSpeed;

                if (timeToCollision > 0 && timeToCollision < shortestTime)
                {
                    shortestTime = timeToCollision;
                    firstTarget = Target;
                    firstMinSeparation = distance - relativeSpeed * shortestTime;
                    firstDistance = distance;
                    firstRelativePos = relativePos;
                    firstRelativeVel = relativeVel;
                }

            }

            //si no hay posibles colisiones
            if (firstTarget == null)
                return Steering;

            //si voy a chocar
            if (firstDistance < radius * 2)
                relativePos = firstRelativePos + firstRelativeVel * shortestTime;

            relativePos.Normalize();
            Steering.linear = relativePos * maxAceleration;

            return Steering;

        }


        //Setiar Align
        public void SetA()
        {
            maxAngularAcceleration = 5.0f;
            slowRadius = 20.0f;
            maxRotation = MathHelper.PiOver4;
            targetRadius = MathHelper.PiOver4/5 ;
            timeToTarget = 0.1f;
        }
        //este alinea la rotacion de el personaje con la del target
        public SteeringOutput Aling(Kinematic Personaje, Kinematic Target)
        {
            SteeringOutput Steering = new SteeringOutput();
            float targetRotation;

            float TO = Target.Orientation;
            float PO = Personaje.Orientation;

            while (TO > MathHelper.Pi)
                TO -= MathHelper.TwoPi;

            while (TO < -MathHelper.Pi)
                TO += MathHelper.TwoPi;

            while (PO > MathHelper.Pi)
                PO -= MathHelper.TwoPi;

            while (PO < -MathHelper.Pi)
                PO += MathHelper.TwoPi;


            float rotation = TO - PO;



            //esta transformacion no me funciona
            //rotation = mapToRange(rotation);

            float rotationSize = Math.Abs(rotation);

            if (rotationSize < targetRadius)
                return Steering;

            if (rotationSize > slowRadius)
                targetRotation = maxRotation;
            else
                targetRotation = maxRotation * rotationSize / slowRadius;

            targetRotation *= rotation / rotationSize;

            Steering.agular = targetRotation - Personaje.Rotation;
            Steering.agular /= timeToTarget;

            float AngularAcceleration = Math.Abs(Steering.agular);

            if (AngularAcceleration > maxAngularAcceleration)
            {
                Steering.agular /= AngularAcceleration;
                Steering.agular *= maxAngularAcceleration;
            }

            Steering.linear = Vector3.Zero;

            return Steering;
        }

        //Setiar Face
        public void SetF()
        {
            SetA();
        }
        //este es que el character ve al target moverse
        public SteeringOutput Face(Kinematic Character, Kinematic Target)
        {
            Vector3 direction = Target.Position - Character.Position;

            if (direction.Length() == 0)
                return new SteeringOutput();

            Kinematic Aux = Target.Clone();

            Aux.Orientation = (float)Math.Atan2(System.Convert.ToDouble(direction.X), System.Convert.ToDouble(direction.Z));

            //Agregado, Pone todos los angulos en positivos
            if (Aux.Orientation < 0)
                Aux.Orientation += MathHelper.TwoPi;

            return Aling(Character, Aux);

        }

        //Setiar LookWhereYouGoing
        public void SetLWTG()
        {
            SetA();
        }
        //este es character consigo mismo, es ver adelante donde se esta moviendo
        public SteeringOutput LookWhereYoureGoing(Kinematic Character, Kinematic Target)
        {
            if (Character.Velocity.Length() == 0)
                return new SteeringOutput();

            Target.Orientation = (float)Math.Atan2(System.Convert.ToDouble(Character.Velocity.X), System.Convert.ToDouble(Character.Velocity.Z));

            return Aling(Character, Target);
        }

        //Setiar Blendes
        public void SetBS()
        {
            maxAceleration = 10.0f;
            maxRotation = MathHelper.PiOver4;
            
        }
        //Combina Comportamientos
        public SteeringOutput BlendedSteering(List<Behavior> Behaviors)
        {
            SteeringOutput Steering = new SteeringOutput();

            foreach (Behavior Behavior in Behaviors)
            {
                Steering.linear.X += Behavior.Steering.linear.X * Behavior.weight;
                Steering.linear.Y += Behavior.Steering.linear.Y * Behavior.weight;
                Steering.linear.Z += Behavior.Steering.linear.Z * Behavior.weight;
                Steering.agular += Behavior.Steering.agular * Behavior.weight; 
                
            }

            if (Steering.linear.Length() > maxAceleration)
            {
                Steering.linear.Normalize();
                Steering.linear.X *= maxAceleration;
                Steering.linear.Y *= maxAceleration;
                Steering.linear.Z *= maxAceleration;
            }

            if (Steering.agular > maxRotation )
                Steering.agular = maxRotation;


            return Steering;
        }

        public SteeringOutput PrioritySteering(List<SteeringOutput> Groups, float epsilon)
        {
            SteeringOutput Steering = new SteeringOutput();
            foreach (SteeringOutput Group in Groups)
            {
                Steering = Group;
                if (Group.linear.Length() > epsilon || Math.Abs(Group.agular) > epsilon)
                    return Steering;
            }
            return Steering;
        }

        private float getNewOrientation(Kinematic Personaje, Vector3 Velocity)
        {
            if (Velocity.Length() > 0)
                return (float)Math.Atan2(System.Convert.ToDouble(Velocity.X), System.Convert.ToDouble(Velocity.Z));
            else
                return Personaje.Orientation;
        }

        private Vector3 asVector(float Angulo)
        {
            return new Vector3((float)Math.Sin(Angulo), 0.0f, (float)Math.Cos(Angulo));

        }

        private double randmBinomial()
        {
            double F = Rand.NextDouble();
            return F;
        }

        private float mapToRange(float rotation)
        {
            /*
            if (rotation > MathHelper.Pi)
                return rotation - 2 * MathHelper.Pi;
            else
                return rotation;
             */

            return rotation - MathHelper.Pi;

        }


        private Vector3 RectIntersect(Vector3 r1, Vector3 r2, float m1, float m2)
        {

            float A = (m1 * r1.X - m2 * r2.X + r2.Z - r1.Z) / (m1 - m2);
            float B = m1 * (A - r1.X) + r1.Z;
            return new Vector3(A, 0.0f, B);
        }


        /*
        float wanderOffset = 5.0f;
        float wanderRadius= 10.0f;
        float wanderRate = MathHelper.PiOver4;
        float wanderOrientation = 0.0f;
        //float maxAcceleration;

        //no funciona y no entiendo lo que hace, tiene algo que ver con ramdom 
        public SteeringOutput Wander(Kinematic Character, Kinematic Target)
        {
            wanderOrientation += randmBinomial() * wanderRate;

            float targetOrientation = wanderOrientation + Character.Orientation;

            Target.Position = Character.Position + wanderOffset * asVector(Character.Orientation);
            Target.Position += wanderRadius * asVector(targetOrientation);

            SteeringOutput Steering = Face(Character, Target);

            Steering.linear = maxAceleration * asVector(Character.Orientation);

            return Steering;

        }

    
        
        public SteeringOutput FollowPath()
        {
            currentParam = path.getParam();

            targetParam = currentParam + pathOffset;

            Target.position = path.getPosition(targetParam);

            return DynamicSeek(Character, Target);
        }
        */

    }
}
